#include "motor.h"
#include "mt6701ct_spi.h"
#include "usart.h"
#include <stdio.h>
#include "stm32f1xx_hal.h"

extern TIM_HandleTypeDef htim1;

// 模拟获取0 - 359°的绝对位置数据，实际应用中需替换为真实获取函数
float get_absolute_position(void) {
    // 这里只是模拟，实际要从位置传感器读取
    float position=MT6701_GetRawAngle();
    return position;
}

int get_commutation_step_new(float angle_deg)
{
    while (angle_deg < 0) angle_deg += 360;
    while (angle_deg >= 360) angle_deg -= 360;

    if (angle_deg < 60) return 1;
    else if (angle_deg < 120) return 2;
    else if (angle_deg < 180) return 3;
			else if (angle_deg < 240) return 4;
    else if (angle_deg < 300) return 5;
    else return 6;
}



// 根据绝对位置确定换向步骤
uint8_t get_commutation_step(float position) {
    // 每个换向步骤对应60°的电角度范围
    return (int)(position / 60) % 6+1;
}

void Motor_start_pwm(){
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
}

void Motor_MX_GPIO_Init(void)
{
    GPIO_InitTypeDef GPIO_InitStruct = {0};

    __HAL_RCC_GPIOA_CLK_ENABLE();

    GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_9 | GPIO_PIN_10;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
}

void Motor_MX_TIM1_Init(void)
{
    TIM_OC_InitTypeDef sConfigOC = {0};
    TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0};

    // 使能定时器 1 时钟
    __HAL_RCC_TIM1_CLK_ENABLE();

    htim1.Instance = TIM1;
    htim1.Init.Prescaler = 0;
    htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim1.Init.Period = 999;  // 调整此值以改变 PWM 频率
    htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    htim1.Init.RepetitionCounter = 0;
    if (HAL_TIM_PWM_Init(&htim1) != HAL_OK)
    {
        Error_Handler();
    }

    sConfigOC.OCMode = TIM_OCMODE_PWM1;
    sConfigOC.Pulse = 0;
    sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
    sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
    sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
    sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
    sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
    if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
    {
        Error_Handler();
    }
    if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
    {
        Error_Handler();
    }
    if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
    {
        Error_Handler();
    }

    sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
    sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
    sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
    sBreakDeadTimeConfig.DeadTime = 0;
    sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
    sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
    sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
    if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK)
    {
        Error_Handler();
    }

    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
}

void set_pwm_duty(TIM_HandleTypeDef *htim, uint32_t channel, float duty)
{
    if (duty < 0) duty = 0;
    if (duty > 1.0f) duty = 1.0f;

    uint32_t pulse = __HAL_TIM_GET_AUTORELOAD(htim) * duty;
    __HAL_TIM_SET_COMPARE(htim, channel, pulse);
    //printf("PWM duty for channel %d: %.2f, Pulse: %d\n", channel, duty, pulse);
}

// 定义不同的换向步骤函数
void BLDC_UW(float duty)
{
    set_pwm_duty(&htim1, TIM_CHANNEL_1, duty);
    set_pwm_duty(&htim1, TIM_CHANNEL_3, 0);
    // 假设 TIM_CHANNEL_1 对应 U 相，TIM_CHANNEL_3 对应 W 相
    // 这里可以根据实际硬件连接调整
    printf("BLDC_UW: U+ W-\n");
}

void BLDC_UV(float duty)
{
    set_pwm_duty(&htim1, TIM_CHANNEL_1, duty);
    set_pwm_duty(&htim1, TIM_CHANNEL_2, 0);
    // 假设 TIM_CHANNEL_1 对应 U 相，TIM_CHANNEL_2 对应 V 相
    printf("BLDC_UV: U+ V-\n");
}

void BLDC_VU(float duty)
{
    set_pwm_duty(&htim1, TIM_CHANNEL_2, duty);
    set_pwm_duty(&htim1, TIM_CHANNEL_1, 0);
    // 假设 TIM_CHANNEL_2 对应 V 相，TIM_CHANNEL_1 对应 U 相
    printf("BLDC_VU: V+ U-\n");
}

void BLDC_VW(float duty)
{
    set_pwm_duty(&htim1, TIM_CHANNEL_2, duty);
    set_pwm_duty(&htim1, TIM_CHANNEL_3, 0);
    // 假设 TIM_CHANNEL_2 对应 V 相，TIM_CHANNEL_3 对应 W 相
    printf("BLDC_VW: V+ W-\n");
}

void BLDC_WV(float duty)
{
    set_pwm_duty(&htim1, TIM_CHANNEL_3, duty);
    set_pwm_duty(&htim1, TIM_CHANNEL_2, 0);
    // 假设 TIM_CHANNEL_3 对应 W 相，TIM_CHANNEL_2 对应 V 相
    printf("BLDC_WV: W+ V-\n");
}

void BLDC_WU(float duty)
{
    set_pwm_duty(&htim1, TIM_CHANNEL_3, duty);
    set_pwm_duty(&htim1, TIM_CHANNEL_1, 0);
    // 假设 TIM_CHANNEL_3 对应 W 相，TIM_CHANNEL_1 对应 U 相
    printf("BLDC_WU: W+ U-\n");
}

// 换向函数
void motor_commutate(uint8_t hall_step, float duty)
{
    // 先全部设置为“高阻”或“低电平”
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_RESET);

    // 所有 PWM 通道关闭
    set_pwm_duty(&htim1, TIM_CHANNEL_1, 0);
    set_pwm_duty(&htim1, TIM_CHANNEL_2, 0);
    set_pwm_duty(&htim1, TIM_CHANNEL_3, 0);

    // 添加死区时间
    HAL_Delay(10);

    switch (hall_step)
    {
        case 1: // U+ W-
            BLDC_UW(duty);
            printf("do step 1\n");
            break;
        case 2: // U+ V-
            BLDC_UV(duty);
            printf("do step 2\n");
            break;
        case 3: // V+ U-
            BLDC_VU(duty);
            printf("do step 3\n");
            break;
        case 4: // V+ W-
            BLDC_VW(duty);
            printf("do step 4\n");
            break;
        case 5: // W+ V-
            BLDC_WV(duty);
            printf("do step 5\n");
            break;
        case 6: // W+ U-
            BLDC_WU(duty);
            printf("do step 6\n");
            break;
        default:
            break;
    }
}